#include <mc_ros/visualization/perception_visualization.h>

PerceptionVisualization::PerceptionVisualization(){
    depthcamera_init_ = false;
    scan_init_ = false;
}

// 设置机器人模型
void PerceptionVisualization::setRobotModel(boost::shared_ptr<ModelBase> model_ptr){
    robotModelPtr_ = model_ptr;
}

// 初始化深度转点云发布者
void PerceptionVisualization::initDepth2PointCloudPublish(string topic_name){
    depth2pointcloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(topic_name, 1);
    depth2pointcloud_raw_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(topic_name+"_raw", 1);
    depthcamera_init_ = true;
}

// 发布原始深度点云
void PerceptionVisualization::publishDepthPointCloudRaw(sensor_msgs::PointCloud2::Ptr depth_pointcloud_ptr){
    depth2pointcloud_raw_pub_.publish(*depth_pointcloud_ptr);    
}

// 发布处理后深度点云
void PerceptionVisualization::publishDepthPointCloud(sensor_msgs::PointCloud2::Ptr depth_pointcloud_ptr){
    depth2pointcloud_pub_.publish(*depth_pointcloud_ptr);    
}

// 初始化雷达转点云发布者
void PerceptionVisualization::initScan2PointCloudPublish(string topic_name){
    scan2pointcloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(topic_name, 1);
    scan2pointcloud_raw_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(topic_name+"_raw", 1);
    scan_init_ = true;
}

// 发布原始雷达点云
void PerceptionVisualization::publishScanPointCloudRaw(sensor_msgs::PointCloud2::Ptr scan_pointcloud_ptr){
    scan2pointcloud_raw_pub_.publish(*scan_pointcloud_ptr);    
}

// 发布处理后雷达点云
void PerceptionVisualization::publishScanPointCloud(sensor_msgs::PointCloud2::Ptr scan_pointcloud_ptr){
    scan2pointcloud_pub_.publish(*scan_pointcloud_ptr);    
}